#include "pointcloud_filter/pointcloud_filter_node.hpp"
#include<pcl_conversions/pcl_conversions.h>

#include<pcl/common/transforms.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include<pcl/filters/passthrough.h>


namespace hnurm {

PointCloudNode::PointCloudNode(const rclcpp::NodeOptions &options)
        : Node("PointCloudNode", options)
{
  RCLCPP_INFO(get_logger(), "PointCloudNode is running");

  lidar_topic_ = this->declare_parameter("lidar_topic","segmentation/obstacle");     //segmentation/obstacle    /livox/lidar/pointcloud
  output_lidar_topic_ = this->declare_parameter("output_lidar_topic_","/pointcloud");
  output_laser_topic_ = this->declare_parameter("output_laser_topic_","/ld_laserscan");
  base_frame_ = this->declare_parameter("base_frame","base_footprint");
  lidar_frame_ = this->declare_parameter("lidar_frame","lidar_link");
  laser_topic_ = this->declare_parameter("laser_topic","/laser/scan");
  laser_frame_ = this->declare_parameter("laser_frame","laser_link");
  robot_radius_ = this->declare_parameter("robot_radius",0.5);

  // tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
  // tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
  pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
    lidar_topic_,
    rclcpp::SensorDataQoS(),
    std::bind(&PointCloudNode::pointcloud_callback,this,std::placeholders::_1)
  );
  laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
    laser_topic_,
    rclcpp::SensorDataQoS(),
    std::bind(&PointCloudNode::laser_callback,this,std::placeholders::_1)
  );
  pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
    output_lidar_topic_,
    rclcpp::SensorDataQoS()
  );
  laser_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(
    output_laser_topic_,
    rclcpp::SensorDataQoS()
  );

}

void PointCloudNode::laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    // 1. 获取 TF 变换（雷达坐标系 -> 车体坐标系）
    // geometry_msgs::msg::TransformStamped transform;
    // try {
    //     transform = tf_buffer_->lookupTransform(
    //         base_frame_,          // 目标坐标系（车体）
    //         msg->header.frame_id,  // 激光雷达坐标系
    //         msg->header.stamp,     // 使用激光数据时间戳
    //         rclcpp::Duration::from_seconds(0.1)
    //     );
    // } catch (const tf2::TransformException &ex) {
    //     RCLCPP_WARN(this->get_logger(), "TF 转换失败: %s", ex.what());
    //     return;
    // }

    // // 2. 构建变换矩阵
    // Eigen::Affine3f lidar_to_base = Eigen::Affine3f::Identity();
    // lidar_to_base.translate(Eigen::Vector3f(
    //     transform.transform.translation.x,
    //     transform.transform.translation.y,
    //     transform.transform.translation.z
    // ));
    // lidar_to_base.rotate(Eigen::Quaternionf(
    //     transform.transform.rotation.w,
    //     transform.transform.rotation.x,
    //     transform.transform.rotation.y,
    //     transform.transform.rotation.z
    // ));

    // 3. 过滤激光数据
    auto filtered_scan = std::make_shared<sensor_msgs::msg::LaserScan>(*msg);
    filtered_scan->ranges.resize(msg->ranges.size());

    for (size_t i = 0; i < msg->ranges.size(); ++i) {
        const float range = msg->ranges[i];
        if (std::isinf(range)) {
            filtered_scan->ranges[i] = range;  // 保留无效值
            continue;
        }

        // 计算激光点在雷达坐标系下的坐标
        const float angle = msg->angle_min + i * msg->angle_increment;
        const float x_lidar = range * std::cos(angle);
        const float y_lidar = range * std::sin(angle);
        const Eigen::Vector3f point_lidar(x_lidar, y_lidar, 0.0f);

        // 转换到车体坐标系
        // const Eigen::Vector3f point_base = lidar_to_base * point_lidar;
        const Eigen::Vector3f point_base = point_lidar;  //不变换到basefootprint 系下，多了一个平移有点怪

        // 计算到车体中心的水平距离
        const float distance = std::hypot(point_base.x(), point_base.y());
        if (distance > robot_radius_) {
            filtered_scan->ranges[i] = range;  // 保留有效点
        } else {
            filtered_scan->ranges[i] = std::numeric_limits<float>::infinity(); // 剔除点
        }
    }

    // 4. 发布过滤后的数据
    filtered_scan->header.frame_id = laser_frame_;  // 更新坐标系
    laser_pub_->publish(*filtered_scan);
}

void PointCloudNode::pointcloud_callback(sensor_msgs::msg::PointCloud2::SharedPtr msg){
  // RCLCPP_INFO(get_logger(),"Received pointcloud message");
  // geometry_msgs::msg::TransformStamped transform;
  // try{
  //   transform = tf_buffer_->lookupTransform(lidar_frame_,base_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));

  // }catch(tf2::TransformException &ex){
  //    RCLCPP_WARN(this->get_logger(), "Transform failed: %s. Waiting for transform...", ex.what());
  //   return;  // 如果转换查找失败，则退出该回调
  // }
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(*msg, cloud);
    // 构建变换矩阵（先旋转后平移）
    // Eigen::Affine3f transform_matrix = Eigen::Affine3f::Identity();
    // transform_matrix.translate(Eigen::Vector3f(
    //     transform.transform.translation.x,
    //     transform.transform.translation.y,
    //     transform.transform.translation.z
    // ));
    // transform_matrix.rotate(Eigen::Quaternionf(
    //     transform.transform.rotation.w,
    //     transform.transform.rotation.x,
    //     transform.transform.rotation.y,
    //     transform.transform.rotation.z
    // ));
    
    // pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
    // pcl::transformPointCloud(cloud, transformed_cloud, transform_matrix);   //取消变换

    //过滤车体半径内的点
    pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
    // for (const auto &point : transformed_cloud) {
    for (const auto &point : cloud) {
        const float distance = std::hypot(point.x, point.y);  
        if (distance > robot_radius_) {
            filtered_cloud.push_back(point);
        }
    }

    //发布过滤后的点云
    sensor_msgs::msg::PointCloud2 filtered_msg;
    pcl::toROSMsg(filtered_cloud, filtered_msg);
    filtered_msg.header.stamp = msg->header.stamp;
    // filtered_msg.header.frame_id = base_frame_;  // 坐标系已转换到车体
    filtered_msg.header.frame_id = lidar_frame_;  // 坐标系已转换到车体
    pointcloud_pub_->publish(filtered_msg);
}

}